/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/*
*  \class	TOccupancyGrid
*  \author 	Jari Saarinen
*  \date 	Mon Jun 12 22:16:05 2006

*  \version: 	0.1  
*/

#include <stdio.h>
#include "TOccupancyGrid.h"

TOccupancyGrid::TOccupancyGrid(){
	initDefaults();
	mallocData();
}
 /**
 * Constructor
 * @p size_x the size of the grid in x-direction, in meters
 * @p size_y the size of the grid in y-direction, in meters
 * @p reso   Resolution. The size of one cell in meters
 * @p origin The position of the grid origin in the world frame of reference
	 **/
// TOccupancyGrid::TOccupancyGrid(float size_x, 
// 															 float size_y, 
// 															 float reso, 
// 															 Grid::TCoord2D origin)
// {
// 	initDefaults();
// 	if(reso<=0){
// 		fprintf(stderr,"TOccupancyGrid::TOccupancyGrid():Resolution must be grater than zero!\n");
// 		exit(1);
// 	}
// 	Nx = (int)(size_x/reso);
// 	Ny = (int)(size_y/reso);
// 	resolution = reso;
// 	world_pos = origin;
// 	information_sum = 0;
// 	mallocData();
// }
/**
 * Constructor
 * @p size_x the size of the grid in x-direction, in meters
 * @p size_y the size of the grid in y-direction, in meters
 * @p reso   Resolution. The size of one cell in meters
 * @p origin The position of the grid origin in the world frame of reference
 **/
TOccupancyGrid::TOccupancyGrid(double size_x, 
															 double size_y, 
															 double reso, 
															 Grid::TCoord2D origin)
{
		initDefaults();
		if(reso<=0){
				fprintf(stderr,"TOccupancyGrid::TOccupancyGrid():Resolution must be grater than zero!\n");
				exit(1);
		}
		Nx = (int)(size_x/reso);
		Ny = (int)(size_y/reso);
		if(Nx%2==0) Nx+=1;
		if(Ny%2==0) Ny+=1;
		
		resolution = reso;
		world_pos = origin;
		information_sum = 0;
		mallocData();
}


/**
 * Constructor with pixels instead of meters
 * @p nx the number of pixels in x-direction
 * @p ny the number of pixels in y-direction
 * @p reso   Resolution. The size of one cell in meters
 * @p origin The position of the grid origin in the world frame of reference
 **/
TOccupancyGrid::TOccupancyGrid(int nx, int ny, double reso, Grid::TCoord2D origin){
		initDefaults();
		if(reso<=0){
				fprintf(stderr,"TOccupancyGrid::TOccupancyGrid():Resolution must be grater than zero!\n");
				exit(1);
		}
		Nx = nx; Ny=ny;
		resolution = reso;
		world_pos = origin;
		information_sum = 0;
		mallocData();
}


TOccupancyGrid::~TOccupancyGrid(){
	if(data) free(data);
	delete input;
	delete dt_out;
}
/**
* Get direct access to the data pointer
* The data is stored in one dimensional data pointer and ordered so that 
* index (i,j)=data[j*Ny+i]
* @return pointer to data;
*/
grid_t *TOccupancyGrid::getDataPtr(){
return data;
}
/**
* Returns the dimension of grid (number of cells in x and y direction)
* @p &x number of cells in x-axis
* @p &y number of cells in y-axis
*/
void TOccupancyGrid::getDim(int &x, int&y){
 x = Nx;
 y = Ny;
}

/**
* Returns the grid position in world coordinate frame
* @return the coordinates of the origin
*/
Grid::TCoord2D TOccupancyGrid::getWorldPos(){
	return world_pos;
}


/**
 * sets the grid position in world coordinate frame
 */
void TOccupancyGrid::setWorldPos(Grid::TCoord2D origin_pose){
		world_pos=origin_pose;
}


/**
* returns the resolution = the number of meters that one cell represents 
*/
float TOccupancyGrid::getResolution(){
	return resolution;
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

/**
* Transform a world coordinate position to 
* pixel coordinate position
* @p P a position in world coordinates
* @return the P in pixel coordinates
*/
Grid::TPixel TOccupancyGrid::toPixel(Grid::TCoord2D P){
	float x,y;
	Grid::TPixel pix;
	///Relative metric coordinates
	x = P.x - world_pos.x;
	y = P.y - world_pos.y;
	
	/// Into pixels
	x /= resolution;
	y /= resolution;
	
	/// The origin is in the centre of grid 
	x += ((float) Nx)/2.0; 
	y = ((float)Ny)/2.0 - y;
	
	pix.x = (int) (x+0.5);
	pix.y = (int) (y+0.5);
	return pix;
}


/**
* Transform pixel coordinate to world coordinate
* @p P a pixel position
* @return the position of the pixel in world coordinates
*/
Grid::TCoord2D TOccupancyGrid::toCoord(Grid::TPixel P){
	Grid::TCoord2D coord;
	float x,y; 

	x = P.x - ((float)Nx)/2.0;
	y = -P.y + ((float)Ny)/2.0; 
	
	coord.x = x * resolution + world_pos.x;
	coord.y = y * resolution + world_pos.y;		
			
	return coord;
}
/**
* Changes an measurement taken in pose (x,y,a) to TCoord2D
* @p x,y,a the robot pose in the world
* @p r,ra the measurement
**/
Grid::TCoord2D TOccupancyGrid::toCoord(float x, float y, float a, float r, float ra){
	Grid::TCoord2D coord;
	
	coord.x = x + r * cos(a+ra);
	coord.y = y + r * sin(a+ra);
	
	return coord;
}
/**
* Set value to pixel position P
* @p P The position in grid_t
* @p value The value that you want to set
* @return true in success, else false
*/
bool TOccupancyGrid::setValue(Grid::TPixel P,grid_t value){
	if(isValid(P)){
		data[P.y*Nx + P.x]=value;
		return true;
	}
	return false;
}

/**
* Sets value to grid in world position P
* @p P a position in world
* @p value the value that you want to set
* @return true in success, else false
*/
bool TOccupancyGrid::setValue(Grid::TCoord2D P, grid_t value){
	Grid::TPixel pix;
	pix = toPixel(P);
	//fprintf(stderr,"Setting to (%.2f%.2f)=(%d,%d)\n",P.x,P.y,pix.x,pix.y);
	if(isValid(pix)){
		data[pix.y*Nx + pix.x]=value;
		return true;
	}
	return false;
}

/**
* get value of the grid from world position P
* @p P a position where you want to take a value
* @p &value the value is copied here
* @return true in success, else false 
**/
bool TOccupancyGrid::getValue(Grid::TCoord2D P, grid_t &value){
	Grid::TPixel pix;
	pix = toPixel(P);
	if(isValid(pix)){
		value = data[pix.y*Nx + pix.x];
		return true;
	}
	//fprintf(stderr,"pix=(%d,%d) N=(%d,%d) ",pix.x,pix.y,Nx,Ny );
	return false;
}

/**
* Gets a value from pixel position P
* @p P the pixel coordinate that you want to retreive the data from
* @p &value The value is copied here
* @return true in success, else false
*/
bool TOccupancyGrid::getValue(Grid::TPixel P, grid_t &value){
	if(isValid(P)){
		value = data[P.y*Nx + P.x];
		return true;
	}
	return false;
}

/**
* Returns index of given pixel
* @p P the pixel coordinate that you want to retreive the data from
* @p &value The value is copied here
* @return true in success, else false
*/
bool TOccupancyGrid::getIndex(Grid::TPixel P, int &value){
	if(isValid(P)){
		value = P.y*Nx + P.x;
		return true;
	}
	return false;
}

/**
* Returns corresponding TPixel of a given grid index
* @p index the grid index you want to match
* @p &P The point is copied here
* ///FIXME?? Modified by Jari 1.2.2011 and not tested
*/
void TOccupancyGrid::getPoint(int index, Grid::TPixel &P){
	P.y = index/Nx;
	P.x = index%Nx;
}



/**
 * Sets the whole grid to given value
 * @p value The wanted grid value
 */
void TOccupancyGrid::setGridToValue(grid_t value){
	for(int i=0;i<Nx*Ny;i++) data[i]=value;
}
	
	
/**
 * Updates the Observation probability @P to the grid position @Pos.
 * @param Pos the coordinate which probability will be updated
 * @param P The observation probability
 * @return true
 */
bool TOccupancyGrid::updateProbability(Grid::TCoord2D Pos, float P){
	float Pold;
	float Pnew;
	grid_t p;
	
	getValue(Pos,p); 				///< To float
	if(p == 0.5) { ///FIXME: Maybe should use PROB_VALUE_MAX/2 but might be difficult
		information_sum++;
	}
	Pold = ((float)p)/(float)PROB_VALUE_MAX; 		///< Old probability
	
	Pnew = ( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P)); 	///< Updated probability
	///Added by Jari to prevent map from going into absolute zero or one
	if(Pnew<0.001) Pnew=0.001;
	if(Pnew>0.999) Pnew=0.999;
	p = (grid_t) (PROB_VALUE_MAX * Pnew); 		///< Back to char
	
	setValue(Pos,p);
	return true;
	
}

/**
 * Updates the Observation probability @P to the grid position @Pos.
 * @param Pos the coordinate which probability will be updated
 * @param P The observation probability
 * @return true
 */
bool TOccupancyGrid::updateProbability(Grid::TPixel Pos, float P){
	float Pold;
	float Pnew;
	grid_t p;
	
	getValue(Pos,p); 				///< To float
	if(p == 0.5) { ///FIXME: Maybe should use PROB_VALUE_MAX/2 but might be difficult
		information_sum++;
	}
	Pold = ((float)p)/(float)PROB_VALUE_MAX; 		///< Old probability
	Pnew = ( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P)); 	///< Updated probability
	///Added by Jari to prevent map from going into absolute zero or one
	if(Pnew<0.001) Pnew=0.001;
	if(Pnew>0.999) Pnew=0.999;
	p = (grid_t) (PROB_VALUE_MAX * Pnew); 		///< Back to char
	//fprintf(stderr,"%.3f %.3f\n",PROB_VALUE_MAX,p);
	
	setValue(Pos,p);
	return true;
	
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/**
* Sets a line to the grid. The line is given as start and end coordinates
* The value that the line is set is given with @value input argument
* @p x1 x-coordinate of the start point
* @p y1 y-coordinate of the start point
* @p x2 x-coordinate of the end point
* @p y2 y-coordinate of the end point
* @p value The value that the line is set to
* @return true in success, else false
**/
bool TOccupancyGrid::setLine(float x1,float y1,float x2,float y2, grid_t value){
	float alpha=atan2(y2-y1,x2-x1);
	float r = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
	
	return setLine(Grid::TCoord2D(x1,y1),r,alpha,value);
}

/**
* Sets a line to grid. The line is given as start point and angle, length pair.
* @p pos the start point of the line
* @p r The length of the line
* @p angle angle of the line
* @p value The value the grid cells under the line is set to
* @p checkRigid Set true if you have rigid structures in map
* @p r_val the value of the rigid cell
* @return true in success, else false
**/
bool TOccupancyGrid::setLine(Grid::TCoord2D pos, 
														 float r, 
														 float angle, 
														 grid_t value,
														 bool checkRigid, 
														 grid_t r_val)
{
	float dl=resolution;
	float length=0;
	float cosa=cos(angle);
	float sina=sin(angle);
	grid_t v=0;
	
	while(length<r){
		if(checkRigid){
			getValue(pos,v);
			if(v!=r_val){
				setValue(pos,value);
			}
		}else{
			setValue(pos,value);
		}
		
		pos.x += dl*cosa;
		pos.y += dl*sina;
		length+=dl;
	}
	return true;
}

/**
 * Sets a line to grid. The line is given as start point and angle, length pair.
 * The grid cell is set to value @value only if it was previously in value @unknown
 * 
 * @p pos the start point of the line
 * @p r The length of the line
 * @p angle angle of the line
 * @p value The value the grid cells under the line is set to
 * @p unknown The value that the grid need to be before it is set to @value
 * @return true in success, else false
 **/
bool TOccupancyGrid::setLine(Grid::TCoord2D pos, 
														 float r, 
														 float angle, 
														 grid_t value, grid_t unknown)
{
		float dl=resolution;
		float length=0;
		float cosa=cos(angle);
		float sina=sin(angle);
		grid_t v=0;
	
		while(length<r){
				getValue(pos,v);
				if(v==unknown){
						setValue(pos,value);
				}
				pos.x += dl*cosa;
				pos.y += dl*sina;
				length+=dl;
		}
		return true;
}

/**
* Add a line to the grid. The line is given as start and end coordinates
* The value that the line is depends on the previous value of grid plus @value input argument
* @p x1 x-coordinate of the start point
* @p y1 y-coordinate of the start point
* @p x2 x-coordinate of the end point
* @p y2 y-coordinate of the end point
* @p value The value that is added to the grid where the line is drawn
* @return true in success, else false
**/
bool TOccupancyGrid::addLine(float x1,float y1,float x2,float y2, grid_t value){
	float alpha=atan2(y2-y1,x2-x1);
	float r = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
	
	return addLine(Grid::TCoord2D(x1,y1),r,alpha,value);
}

/**
 * Adds a line to grid. The line is given as start point and angle, length pair.
 * The grid cell is set to previous value plus @value
 * 
 * @p pos the start point of the line
 * @p r The length of the line
 * @p angle angle of the line
 * @p value The value added to the grid cells under the line
 * @return true in success, else false
 **/
bool TOccupancyGrid::addLine(Grid::TCoord2D pos, 
														 float r, 
														 float angle, 
														 grid_t value)
{
		float dl=resolution;
		float length=0;
		float cosa=cos(angle);
		float sina=sin(angle);
		grid_t v=0;
	
		while(length<r){
				getValue(pos,v);
				setValue(pos,value+v);
				pos.x += dl*cosa;
				pos.y += dl*sina;
				length+=dl;
		}
		return true;
}


/**
* updates probability on a line to grid. 
* The line is given as start point and angle, length pair.
* @p pos the start point of the line
* @p r The length of the line
* @p angle angle of the line
* @p P The observation probability
* @return true in success, else false
**/
bool TOccupancyGrid::setBayesLine(Grid::TCoord2D pos, float r, float angle, float P){
	float dl=resolution;
	float length=0;
	float cosa=cos(angle);
	float sina=sin(angle);
	
	Grid::TPixel pix = toPixel(pos);
	Grid::TPixel p;
	int ind=0;
	while(length<=(r+resolution/2.0)){
		p.x = pix.x + (int)(ind*cosa);
		p.y = pix.y - (int)(ind*sina);
		
		updateProbability(p, P);
		length+=resolution;
		ind++;
	}
	return true;	
}
	

/**
* Sets a beam into the grid. By beam I mean a range measurement that has 
* a limited beam width (not only a straigth line). e.g. SICK laser scanner typically has
* beam width of 0.5degrees, the outdoor version has about 0.75degrees.  
* @p pos the start position of the beam
* @p r the length of the beam
* @p angle the angle of the beam centre
* @p beamWidth How wide is the beam
* @p value to which value the grid is set to.
* @return true in success, else false
*/
bool TOccupancyGrid::setBeam(Grid::TCoord2D pos, float r, float angle,float beamWidth, grid_t value){
	float width;
	float a = angle-beamWidth/2.0; ///<Beam start angle
	float da;
	//This is approximately how wide the beam is in the end of beam [m]
	width = 2.0 * sin(beamWidth/2.0)*r;
	
	float numA = width/resolution; ///< This is how many lines we need
	da = beamWidth/numA; ///< Delta angle
	
	///Draws the beam as lines. This is not very efficient algorithm.
	while(a<=(angle+beamWidth/2.0)){
		setLine(pos,r,a,value);
		a+=da;
	}
	
	return true;
}

/**
 * Sets an arc using normal distribution
 * @p pos pos of the robot - start of the beam
 * @p beamangle the total beam angle of the sensor
 * @p distance the ranging information in m
 * @p angle The start angle of the beam 
 * @return true in success, else false
 */
bool TOccupancyGrid::setNormalDistributionArc(Grid::TCoord2D pos, float beam_angle, float distance, float angle){
	
	/****Calculation of the probability for the ocupancy grid****
	 * Pref represents the probability along the arc where the hit was detected 
	 * These are the maximum probabilities for the calculation of each line for the beam
	 * using normal distribution:
	 * P=1/(o*sqrt(2*M_PI)) * exp(-0.5*(((x-EW)/o))*(((x-EW)/o)))
	 * with o=standard deviation and EW=mean
	 * ******************************************************************/
	
	float r=distance;
	float a=angle; ///< start angle
	
	float arc_length=beam_angle*r; ///< length of the arc at the hitpoint		
	int numA = round(arc_length/resolution); ///< This is how many lines we need
	
	float Pref[numA+1], P;
	int i=0,j=0;
	float EWarc=angle+beam_angle; ///< mean of the distribution along the arc
	float EWbeam=distance; ///< mean of the distribution along the beam
	
	float oarc=beam_angle/2.0; ///< standard deviation for distribution along the arc
	float obeam=0.01; ///< standard deviation for distribution along the beam 0.1 - 0.5m
	float Varc=(oarc*sqrt(2*M_PI)*(OCC_PROB_SONAR-EMPTY_PROB)); ///< Scaling factor for the distribution (arc)
	float Vbeam; ///< Scaling factor for the distribution (beam)
	
	float da=beam_angle/numA; ///< Delta angle
	float dr=(4*obeam)/numA; ///< Delta range
	
	Grid::TPixel pix(0,0) ;
	std::set<Grid::TPixel,Grid::PixelClassCompare> visited;
	
	for(j=0;j<(numA+1);j++){
		/**the following corrects the measurement above an angle of +-15° referred to the center of the beam**
		 * based on measurements, which showed that one gets 1cm more compared to the center
		 * only tested with the SRF08*/
		if((abs((abs(angle+(beam_angle/2.0))-abs(a)))>=oarc)&&(beam_angle>=(40.0/180.0*M_PI)))
			 r=distance-0.01;
		else r=distance;
		
		/***convert the carthesian coordinate to pixel***/								
		pix=toPixel(Grid::TCoord2D (r*cos(a)+pos.x,r*sin(a)+pos.y));
		
		/**Store the reference probabilities in an array**/			
		Pref[j]=OCC_PROB_SONAR; ///< might be better because of that the echo can origin from everywhere along the arc
		//Pref[j]=(Varc*(1.0/(oarc*sqrt(2*M_PI)))*(exp(-0.5*(((a-EWarc)/oarc))*(((a-EWarc)/oarc)))))+EMPTY_PROB;
		
		/***check whether the pixel was already updated due to conversation from carthesian coordinates to pixel***/
		if (visited.count(Grid::TPixel(pix.x,pix.y))==0){
		  visited.insert(Grid::TPixel(pix.x,pix.y));
		  updateProbability(Grid::TPixel(pix.x,pix.y), Pref[j]); ///< should be used for ubdate the global map directly
		  }
							
		a +=da;
	}
	
	/*** normal distribution along a beam line
	 * each line in the arrays represent one beam line*/
	a=angle;
	for(i=1;i<(numA+2);i++){
		if((abs((abs(angle+(beam_angle/2.0))-abs(a)))>=oarc)&&(beam_angle>=(40.0/180.0*M_PI)))
			 r=(distance-0.01)-3*obeam;
		else r=distance-3*obeam;
		
		Vbeam=(obeam*(sqrt(2*M_PI))*(Pref[i-1]-EMPTY_PROB));
		
		for(j=0;j<(numA+1);j++){
			
			pix=toPixel(Grid::TCoord2D (r*cos(a)+pos.x,r*sin(a)+pos.y));
								
			P=(Vbeam*(1.0/(obeam*sqrt(2*M_PI)))*(exp((-0.5)*(((r-EWbeam)/obeam))*(((r-EWbeam)/obeam)))))+EMPTY_PROB;
				
			if (visited.count(Grid::TPixel(pix.x,pix.y))==0){
			  visited.insert(Grid::TPixel(pix.x,pix.y));
			  updateProbability(Grid::TPixel(pix.x,pix.y), P);
			  }			
					
			r +=dr;
		}
		a +=da;
	}
	
	return true;
}

/**
 * Sets a beam using normal distribution
 * @p pos pos of the robot - start of the beam
 * @p beamangle the total beam angle of the sensor
 * @p distance the ranging information in m
 * @p angle The start angle of the beam 
 * @return true in success, else false
 */
bool TOccupancyGrid::setNormalDistributionBeam(Grid::TCoord2D pos, float beam_angle, float distance, float angle){
	
	/****Calculation of the probability for the ocupancy grid****
	 * Pref represents the probability along the arc where the hit was detected 
	 * These are the maximum probabilities for the calculation of each line for the beam
	 * using normal distribution:
	 * P=1/(o*sqrt(2*M_PI)) * exp(-0.5*(((x-EW)/o))*(((x-EW)/o)))
	 * with o=standard deviation and EW=mean
	 * ******************************************************************/
	
	float r=distance;
	float a=angle; ///< start angle
	
	float arc_length=beam_angle*r; ///< length of the arc at the hitpoint		
	int numA = round(arc_length/resolution); ///< This is how many lines we need
	int numB = round(distance/resolution); ///< This is how many lines we need
	
	float Pref[numA+1], P;
	int i=0,j=0;
	float EWarc=angle+beam_angle/2.0; ///< mean of the distribution along the arc
	float EWbeam=distance; ///< mean of the distribution along the beam
	
	float oarc=beam_angle; ///< standard deviation for distribution along the arc
	float obeam=0.01; ///< standard deviation for distribution along the beam 0.1 - 0.5m
	float Varc=(oarc*sqrt(2*M_PI)*(OCC_PROB_SONAR-EMPTY_PROB)); ///< Scaling factor for the distribution (arc)
	float Vbeam; ///< Scaling factor for the distribution (beam)
	
	float da=beam_angle/numA; ///< Delta angle
	float dr=(distance+obeam)/numB; ///< Delta range
	
	Grid::TPixel pix(0,0) ;
	std::set<Grid::TPixel,Grid::PixelClassCompare> visited;
	
	
	 
	for(j=0;j<(numA+1);j++){
		/**the following corrects the measurement above an angle of +-15° referred to the center of the beam**
		 * based on measurements, which showed that one gets 1cm more compared to the center
		 * only tested with the SRF08*/
		if((abs((abs(angle+(beam_angle/2.0))-abs(a)))>=oarc)&&(beam_angle>=(40.0/180.0*M_PI)))
			 r=distance-0.01;
		else r=distance;
		
		/***convert the carthesian coordinate to pixel***/							
		pix=toPixel(Grid::TCoord2D (r*cos(a)+pos.x,r*sin(a)+pos.y));
		
		/**Store the reference probabilities in an array**/			
		Pref[j]=OCC_PROB_SONAR; ///< might be better because of that the echo can origin from everywhere along the arc
		//Pref[j]=(Varc*(1.0/(oarc*sqrt(2*M_PI)))*(exp(-0.5*(((a-EWarc)/oarc))*(((a-EWarc)/oarc)))))+EMPTY_PROB;
		
		/***check whether the pixel was already updated due to conversation from carthesian coordinates to pixel***/
		if (visited.count(Grid::TPixel(pix.x,pix.y))==0){
		  visited.insert(Grid::TPixel(pix.x,pix.y));
		  updateProbability(Grid::TPixel(pix.x,pix.y), Pref[j]);
		  }
							
		a +=da;
	}
	
	/**Set the other lines in the array**
	 * normal distribution along a beam line
	 * each line in the arrays represent one beam line*/
	a=angle;
	for(i=1;i<(numA+2);i++){
			
		r=0;
		Vbeam=(obeam*(sqrt(2*M_PI))*(Pref[i-1]-EMPTY_PROB));
		
		for(j=0;j<(numB+1);j++){
			
			pix=toPixel(Grid::TCoord2D (r*cos(a)+pos.x,r*sin(a)+pos.y));
			
			P=(Vbeam*(1.0/(obeam*sqrt(2*M_PI)))*(exp((-0.5)*(((r-EWbeam)/obeam))*(((r-EWbeam)/obeam)))))+EMPTY_PROB;
				
			if (visited.count(Grid::TPixel(pix.x,pix.y))==0){
			  visited.insert(Grid::TPixel(pix.x,pix.y));
			  updateProbability(Grid::TPixel(pix.x,pix.y), P);
			  }			
					
			r +=dr;
		}
		a +=da;
	}
	
	return true;
}

/**
* Set a circle to map 
* @p pos the position of the centre of the circle
* @p r The radius of the circle
* @p value To which value the grid is set to. 
* @return true in success, else false
*/
bool TOccupancyGrid::setCircle(Grid::TCoord2D pos, float r, grid_t value){
	Grid::TPixel pix = toPixel(pos);
	int radius = int (r/resolution);
	
		// set pixels (i,j) in image corresponding to given circle parameters
	int i, j, diff2, diff;
	for (i = pix.x - radius; i <= pix.x + radius; ++i) {
		diff2 = (radius * radius) - ((i - pix.x) * (i - pix.x));
		diff = (int) sqrt(((double) diff2));
		j = pix.y - diff;
		setValue(Grid::TPixel(i,j),value);
		j = pix.y + diff;
		setValue(Grid::TPixel(i,j),value);
	}
	
	return true;
}


/**
 * Sets a an arc into the grid. The arc is defined the same way as beam, but
 * now only the only the values at the end of @r are set (i.e arc) 
 * @p pos the start position of the beam
 * @p r the length of the beam
 * @p angle the angle of the beam centre
 * @p beamWidth How wide is the beam
 * @p value to which value the grid is set to.
 * @return true in success, else false
 */
bool TOccupancyGrid::setArc(Grid::TCoord2D pos, 
														float r, 
														float angle,
														float beamWidth, 
														grid_t value)
{
	float width;
	float a = angle-beamWidth/2.0; ///<Beam start angle
	float da;
	float x,y;
	
	//This is approximately how wide the beam is in the end of beam [m]
	width = 2.0 * sin(beamWidth/2.0)*r;
	
	float numA = width/resolution; ///< This is how many lines we need
	da = beamWidth/numA; ///< Delta angle
	
	///Draws the beam as lines. This is not very efficient algorithm.
	while(a<=(angle+beamWidth/2.0)){
		x = pos.x + r*cos(a);
		y = pos.y + r*sin(a);
		setValue(Grid::TCoord2D(x,y),value);
		a+=da;
	}

return true;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/**
* This method will set the scan end points to grid 
* The (x,y,a) is the robot/sensor Pose in the world
* The s contains the scan. For each point there is a (r,a)-pair which defines the 
* range and angle with respect to robot of each measurement.
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p value The value the scan points are set.
* @return true in success, else false
*/
bool TOccupancyGrid::setScanPoints(float x, float y ,float a, Grid::TScan &s, grid_t value){
	for(int i=0;i<s.N;i++){
			if(s.r[i]>0) setValue(toCoord(x,y,a,s.r[i],s.a[i]),value);
	}
	return true;
}	

/**
 * This method will set the scan end points to grid 
 * The grid is set to @value only if the previous value was @unknown
 *
 * The (x,y,a) is the robot/sensor Pose in the world
 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
 * range and angle with respect to robot of each measurement.
 * @p x x-coordinate of the robot
 * @p y y-coordinate of the robot
 * @p a heading of the robot
 * @p s The scan
 * @p value The value the scan points are set.
 * @p unknown The value that the grid must be in order for the value to be set.
 * @return true in success, else false
 */
bool TOccupancyGrid::setScanPoints(float x, float y ,float a, Grid::TScan &s, grid_t value, grid_t unknown){
		grid_t v;
		for(int i=0;i<s.N;i++){
				if(s.r[i]>0){
						Grid::TCoord2D p = toCoord(x,y,a,s.r[i],s.a[i]);
						getValue(p,v);
						if(v == unknown)
								setValue(p,value);
				}
		}
		return true;
}	


/**
* This method will fill the scan to grid. It means that the endpoints are set as 
* occupied (o_val) and the points that don't contain the obstacles are set as 
* empty (e_val)
*
* The (x,y,a) is the robot/sensor Pose in the world
* The s contains the scan. For each point there is a (r,a)-pair which defines the 
* range and angle with respect to robot of each measurement.
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p e_val The value the scan hit points are set = occupied
* @p o_val The value for empty cell
* @return true in success, else false
*/
bool TOccupancyGrid::setScanFillSimple(
																			float x, 
																			float y ,
																			float a, 
																			Grid::TScan &s, 
																			grid_t e_val,
																			grid_t o_val)
{
	for(int i=0;i<s.N;i++){
		if(s.r[i]>0)
				setLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,e_val);
	}
	setScanPoints(x,y,a,s,o_val);
	return true;
}	

/**
 * This method will fill the scan to grid. It means that the endpoints are set as 
 * occupied (o_val) and the points that don't contain the obstacles are set as 
 * empty (e_val)
 * 
 * Additionally this method checks if the cell is in the value @unknown. If not, 
 * it is not updated.  
 *
 * The (x,y,a) is the robot/sensor Pose in the world
 * The s contains the scan. For each point there is a (r,a)-pair which defines the 
 * range and angle with respect to robot of each measurement.
 * @p x x-coordinate of the robot
 * @p y y-coordinate of the robot
 * @p a heading of the robot
 * @p s The scan
 * @p e_val The value the scan hit points are set = occupied
 * @p o_val The value for empty cell
 * @p u_val Value of unknown cell
 * @return true in success, else false
 */
bool TOccupancyGrid::setScanFillSimple(float x, 
												 float y ,
												 float a, 
												 Grid::TScan &s, 
												 grid_t e_val,
												 grid_t o_val,
												 grid_t u_val)
{
		for(int i=0;i<s.N;i++){
				if(s.r[i]>0)
						setLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,e_val,u_val);
		}
		setScanPoints(x,y,a,s,o_val,u_val);
		return true;
}


/**
* This method is the same as setScanFillSimple(), but instead of simply
* setting the grid cells into some value, the cell probabilities are updated
* with bayes formula. 
* This method uses define PROB_VALUE_MAX. This value can be used if 
* you have e.g. grid_t set to char (use PROB_VALUE_MAX 255)
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p Pe empty measurement probability
* @p Po occupied probability 
* @return true in success, else false
**/
bool TOccupancyGrid::setScanFillProb(float x, 
																		 float y ,
																		 float a, 
																		 Grid::TScan &s,
																		 float Pe,
																		 float Po)
{
	for(int i=0;i<s.N;i++){
		if(s.r[i]>0){
				setBayesLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,Pe);
				updateProbability(toCoord(x,y,a,s.r[i],s.a[i]),Po);
		}
	}	
	return true;
}


/**
 * This method is the same as setScanFillSimple(), but instead of simply
 * setting the grid cells into some value, the cell probabilities are updated
 * with bayes formula. 
 * The method is essentially the same as the one above, but with an exception that 
 * this method uses @max_dist to determine if the scan is longer. 
 * If the scan value is longer, then the area between x,y to @max_dist is set to empty value
 * This method uses define PROB_VALUE_MAX. This value can be used if 
 * you have e.g. grid_t set to char (use PROB_VALUE_MAX 255)
 * @p x x-coordinate of the robot
 * @p y y-coordinate of the robot
 * @p a heading of the robot
 * @p s The scan
 * @p Pe empty measurement probability
 * @p Po occupied probability 
 * @param max_dist The maximum distance for hit to be recorded into the grid
 * @return true in success, else false
 **/
bool TOccupancyGrid::setScanFillProb(float x, 
																		 float y ,
																		 float a, 
																		 Grid::TScan &s,
																		 float Pe,
																		 float Po,
																		 float max_dist)
{
		for(int i=0;i<s.N;i++){
				if(s.r[i]>max_dist){
						setBayesLine(Grid::TCoord2D(x,y),max_dist,s.a[i]+a,Pe);	
				}else	if(s.r[i]>0){
						setBayesLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,Pe);
						updateProbability(toCoord(x,y,a,s.r[i],s.a[i]),Po);
				}
		}	
		return true;
}



/**
* Returns measurement likelihood given a pose (x,y,a)
* @param x,y,a pose where the measurement is taken from
* @param &s scan = measurement
* @param correct the number of correct indexes
* @param wrong number of wrong indexes
**/
float TOccupancyGrid::getMeasurementLikelihood(float x, float y, float a,Grid::TScan &s, int &correct, int &wrong){
	float dl=resolution;
	float length=0;
	Grid::TPixel pix = toPixel(Grid::TCoord2D(x,y));
	Grid::TPixel p;
	int ind=0;
	float r;
	float angle;
	float corr = 0;
	grid_t v;
	correct=0,wrong = 0;
	
	for(int i=0;i<s.N;i++){
		r = s.r[i];
		angle = s.a[i]+a;
		float cosa=cos(angle);
		float sina=sin(angle);
	
		while(length<r){ ///All these have empty prob 
                        p.x = pix.x + (int)(ind*cosa);
			p.y = pix.y - (int)(ind*sina);
			if(getValue(p,v)){
				if(fabs(v-0.5)>0.1){
					corr += (v)*(v);
					if(fabs(v)>0.5) wrong++;
					else correct++;
				}
			}
	
			length+=resolution;
			ind++;
		}
		p.x = pix.x + (int)(ind*cosa);
		p.y = pix.y - (int)(ind*sina);
		
		if(getValue(p,v)){
			if(fabs(v-0.5)>0.1){
				corr += (v-1.0)*(v-1.0);
				if(1.0-v>0.5) wrong++;
				else correct++;
			}
		}
		
	}
	return corr;	
}

	
	
	
/**
* This is the same method as setScanFillSimple(), but this method will not changed
* the rigid structures in the map i.e. if the cell is in the r_val, then it will not beam
* updated, no matter what the measurement says.
*
* The (x,y,a) is the robot/sensor Pose in the world
* The s contains the scan. For each point there is a (r,a)-pair which defines the 
* range and angle with respect to robot of each measurement.
* @p x x-coordinate of the robot
* @p y y-coordinate of the robot
* @p a heading of the robot
* @p s The scan
* @p e_val The value the scan hit points are set = occupied
* @p o_val The value for empty cell
* @p r_val The value for the rigid obstacle
* @return true in success, else false
*/
bool TOccupancyGrid::setScanFillWithRigidWalls(float x, 
															float y ,
															float a, 
															Grid::TScan &s, 
															grid_t e_val,
															grid_t o_val,
															grid_t r_val)
{
	grid_t v=0;
	
	for(int i=0;i<s.N;i++){
		setLine(Grid::TCoord2D(x,y),s.r[i]-resolution,s.a[i]+a,e_val,true,r_val);
		
		getValue(toCoord(x,y,a,s.r[i],s.a[i]),v);
		if(v!=r_val){
			setValue(toCoord(x,y,a,s.r[i],s.a[i]),o_val);
		}
	}
	
	return true;
}	


///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////


/**
* Copies the grid data to RGB pointer
* The value of cell is divided with @scale
* The method fills the RGB as grey scale image 
* * The rgb pointer must be reserved. Size is expected to be 3*Nx*Ny
* @p *rgb pointer to RGB data
* @p scale divider for the grid data
* @return true
*/
bool TOccupancyGrid::fillRGB(unsigned char *rgb, float scale){
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			rgb[3*Nx*j+3*i] = (unsigned char) (data[j*Nx+i]/scale);
			rgb[3*Nx*j+3*i+1] = (unsigned char) (data[j*Nx+i]/scale);
			rgb[3*Nx*j+3*i+2] = (unsigned char) (data[j*Nx+i]/scale);
		}
	}
	return true;
}
/**
	* Copies the grid data to RGB pointer
	* The method uses empty, occupied and rigid values to 
	* set R=rigid,G=free and B=occupied values
	* If the value is none of the above, it is set to black
	* The rgb pointer must be reserved. Size is expected to be 3*Nx*Ny			
	* 
	* @p *rgb The RGB data pointer
	* @p e_val Value for empty=free cells
	* @p o_val Value for occupied cells
	* @p r_val Value for Rigid cells
	* @return true
	*/
bool TOccupancyGrid::fillRGB(unsigned char *rgb, grid_t e_val, grid_t o_val, grid_t r_val){
	grid_t v=0;
	unsigned char r,g,b;
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			getValue(Grid::TPixel(i,j),v);
			if(v==e_val){
				r = 0; g=255; b = 0;
			}else if(v==o_val){
				r = 0; g=0; b = 255;
			}
			else if(v==r_val){
				r = 255; g=0; b = 0;
			}else{
				r = 0; g=0; b = 0;
			}
			rgb[3*Nx*j+3*i] = r;
			rgb[3*Nx*j+3*i+1] = g;
			rgb[3*Nx*j+3*i+2] = b;
		}
	}
	return true;
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/**
* Returns a line taken from position (x,y) to direction a
* The line ends when cell value in direction a is equal or grater than
* o_val. Method returns the metric distance.
* @p x world coordinate x
* @p y world coordinate y
* @p a direction of line
* @return the range. -1 is returned if no hit.
**/
float TOccupancyGrid::getLine(float x,float y, float a,grid_t o_val){
	float dl=resolution;
	float length=0;
	float cosa=cos(a);
	float sina=sin(a);
	Grid::TCoord2D pos(x,y);
	grid_t v;
	while(1){
		
		if(!getValue(pos,v)){
			//fprintf(stderr,"Error l:%.2f Pos=(%.2f, %.2f)\n", length,pos.x,pos.y);
			return -1;
		}
		if(v>=o_val){
			return (length);
		}
		pos.x += dl*cosa;
		pos.y += dl*sina;
		length+=dl;
		
		//fprintf(stderr,"%.2f ", length);
	}
	return -1;
}

/**
 * Returns a line taken from position (x,y) to direction a
 * The line ends when cell value in direction a is equal or grater than
 * o_val. Method returns the metric distance.
 * @p x world coordinate x
 * @p y world coordinate y
 * @p a direction of line
 * @p o_val threshold
 * @p *dt disance transformation matrix 
 * @return the range. -1 is returned if no hit.
 **/
float TOccupancyGrid::getLine(float x,float y, float a,grid_t o_val, float *dt){
	float dl=resolution;
	float length=0;
	float cosa=cos(a);
	float sina=sin(a);
	Grid::TCoord2D pos(x,y);
	grid_t v;
	Grid::TPixel pix;
	while(1){
		pix = toPixel(pos);
		if(!getValue(pix,v)){
			//fprintf(stderr,"Error l:%.2f Pos=(%.2f, %.2f)\n", length,pos.x,pos.y);
			return -1;
		}
		if(v>=o_val){
			return (length);
		}
		dl = dt[pix.y*Nx+pix.x];
		pos.x += dl*cosa;
		pos.y += dl*sina;
		if(dl<resolution) dl = resolution;
		length+=dl;
		if(length > 80) return -1;
		//fprintf(stderr,"%.2f ", length);
	}
	return -1;
}



/**
	* returns a scan taken from (x,y). The first measurement is taken from angle @a 
	* @n is the number of measurements taken and @da is the angle difference between
	* measurements. The result is stored to @s. If the s.N != n then it is reallocated.
	* The o_val is used to check if the cell has obstacle
	* @p x world coordinate x
	* @p y world coordinate y
	* @p a direction of the first measurement
	* @p n number of measurements taken
	* @p da the angle difference between the measurements 
	* @p o_val The threshold for checking if the cell has obstacle
	* @p s The scan s.r[] will be -1 if no measurement can be taken 
	* @return true
	**/
bool TOccupancyGrid::getScan(float x,
														 float y, 
														 float a, 
														 int n, 
														 float da, 
														 grid_t o_val, 
														 Grid::TScan &s)
{
	float angle=a;
	float ang = 0;
	if(s.N != n){
		s.alloc(n);
	}
	
	for(int i = 0;i<n;i++){
		s.a[i] = ang;
		s.r[i] = getLine(x,y,angle,o_val);
		angle += da;
		ang+=da;
	}
	return true;
}

/**
* Makes a distance transform for the grid. The algorithm uses binary image, so 
* th is used to define if the cell is occupied or not
* @p *out Nx*Ny sized pointer that the distances are stored into 
* @p th Threshold for segmentation
* @return maximum distance
**/
float TOccupancyGrid::distanceTransform(float *out, grid_t th){
	float maxd=0;
	int indx,indy;
	/**
	for(int y=0;y<Nx;y++){
		for(int x=0;x<Nx;x++){
			if(data[Nx*y+x]<th) imRef(input,x,y) = 0;
			else imRef(input,x,y) = 1;
		}
	}
	**/
	for(int y=0;y<Ny;y++){
		for(int x=0;x<Nx;x++){
			if(data[Nx*y+x]<th) imRef(dt_out,x,y) = INF;
			else imRef(dt_out,x,y) = 0;
		}
	}
	
	//image<float> *output = dt(input);
	dt2(dt_out);
	/*
	for(int y=0;y<Nx;y++){
		for(int x=0;x<Nx;x++){
			out[y*Nx+x] = sqrt(imRef(output,x,y))*resolution;
			if(out[y*Ny+x] > maxd){
				maxd = out[y*Ny+x];
				indx = x;
				indy = y;
			}
		}
	}
	*/
	for(int y=0;y<Ny;y++){
			for(int x=0;x<Nx;x++){
					out[y*Nx+x] = sqrt(imRef(dt_out,x,y))*resolution;
					//out[y*Nx+x] = imRef(dt_out,x,y)*resolution;
					if(out[y*Nx+x] > maxd){
							maxd = out[y*Nx+x];
							indx = x;
							indy = y;
					}

			}
	}
	
	//delete output;
					
	return maxd;
}	 

/**
 * Makes a Squared distance transform for the grid. The algorithm uses binary image, so 
 * th is used to define if the cell is occupied or not
 * The output is a squared _pixel_ distanceTransform of the map
 * @p *out Nx*Ny sized pointer that the distances are stored into 
 * @p th Threshold for segmentation
 **/
void TOccupancyGrid::distance2Transform(float *out, grid_t th){
		float maxd=0;
		int indx,indy;

		for(int y=0;y<Nx;y++){
			for(int x=0;x<Nx;x++){
				if(data[Nx*y+x]<th) imRef(dt_out,x,y) = INF;
				else imRef(dt_out,x,y) = 0;
			}
		}

		dt2(dt_out);
		for(int y=0;y<Nx;y++){
			for(int x=0;x<Nx;x++){
					out[y*Nx+x] = imRef(dt_out,x,y);
				}
		}
}	 



/**
* This is the same method as above one, but it uses a distance transformation
* matrix to make the calculation more efficient
* returns a scan taken from (x,y). The first measurement is taken from angle @a 
* @n is the number of measurements taken and @da is the angle difference between
* measurements. The result is stored to @s. If the s.N != n then it is reallocated.
* The o_val is used to check if the cell has obstacle
* 
* @p x world coordinate x
* @p y world coordinate y
* @p a direction of the first measurement
* @p n number of measurements taken
* @p da the angle difference between the measurements 
* @p o_val The threshold for checking if the cell has obstacle
* @p *dt distance transform matrix calculated with distanceTransform()
* @p s The scan s.r[] will be -1 if no measurement can be taken 
* @return true
**/
bool TOccupancyGrid::getScan(float x,
							 float y, 
							 float a, 
							 int n, 
							 float da, 
							 grid_t o_val,
							 float *dt, 
							 Grid::TScan &s)
{
	float angle=a;
	float ang=0;
	if(s.N != n){
		s.alloc(n);
	}
	
	for(int i = 0;i<n;i++){
		s.a[i] = ang;
		s.r[i] = getLine(x,y,angle,o_val,dt);
		angle += da;
		ang+=da;
	}
	
	return true;
}


/**
* Get submap (copy) from the grid.
* The centre of the submap is @p centre. The left upper corner is centre.x-Nx/2,centre.y-Ny/2
* @param *submap The submap is copied here. Must be reserved size (nx*ny)
* @param centre The "center" pixel around which the submap is taken from
* @param nx size of submap in x-direction
* @param ny size of the submap in y-dir.
*/
bool TOccupancyGrid::getSubmap(float *submap, Grid::TPixel centre, int nx, int ny){
	int row_begin = centre.x - nx/2;
	int col_begin = centre.y - ny/2;
	int gridx,gridy;
	
	if(nx<0 || ny<0||nx>Nx||ny>Ny||submap == NULL|| !isValid(centre)) return false;
	
	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			gridx =(row_begin+i);
			gridy= (col_begin+j);
			if(isValid(Grid::TPixel(gridx,gridy))){
				submap[nx*j+i]=data[gridy*Nx+gridx];
			}else submap[nx*j+i] = 0.5;
		}	
	}
	return true;
}

/**
 * Set submap (copy) to the grid.
 * The centre of the submap is @p centre. The left upper corner is centre.x-Nx/2,centre.y-Ny/2
 * @param *submap The submap is copied here. Must be reserved size (nx*ny)
 * @param centre The "center" pixel around which the submap is taken from
 * @param nx size of submap in x-direction
 * @param ny size of the submap in y-dir.
 */
bool TOccupancyGrid::setSubmap(float *submap, Grid::TPixel centre, int nx, int ny){
	int row_begin = centre.x - nx/2;
	int col_begin = centre.y - ny/2;
	int gridx,gridy;
	
	if(nx<0 || ny<0||nx>Nx||ny>Ny||submap == NULL|| !isValid(centre)) return false;
	
	for(int j=0;j<ny;j++){
		for(int i=0;i<nx;i++){
			gridx =(row_begin+i);
			gridy= (col_begin+j);
			if(isValid(Grid::TPixel(gridx,gridy))){
				data[gridy*Nx+gridx]=submap[nx*j+i];
			}	
		}	
	}
	return true;
}




/**
* Returns the minimum of the surroundig of pixel in pos (x,y)
* @p x x index in grid
* @p y y index in grid
* @return the minimum value around (x,y)
*/
grid_t TOccupancyGrid::getMin(int x, int y){
  grid_t min_val=10000;
	int cnt = 0;
	int num;

	num = 1;
	  /// X-coordinates
	for(int i=x-num;i<x + num ; i++ ){
		// Y-coordinates
		for(int j=y -num ;j<y + num;j++){
			
			if(i>= 0 && j>=0 && i<Nx && j<=Ny){
				if(i == x && j==y  ){
				}
				else{
					grid_t val;
					if(!(x<0 || x>=Nx || y<0 || y>=Ny)){
						val = data[j*Ny+i];
						if(val<min_val) min_val = val;
						cnt++;
					}
				}
			}
		}
	}/// For x
	
	if(cnt == 0) return data[y*Ny+x];
	
	return min_val;
}
/**
 * Computes a gradient of the grid. 
 * The method computes the gradient as [value-(minimum of neighbour cells)]/resolution 
 * @param *out Output pointer. If null the memory is reserved
 * @return maximum grid value
 **/
float TOccupancyGrid::gradient(grid_t *out){
  float maxv=-10000;
	if(out==NULL) out = (grid_t *) malloc(Nx*Ny*sizeof(grid_t));
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			out[j*Ny+i] = (data[j*Ny+i] - getMin(i,j))/resolution;
			if(out[j*Ny+i] > maxv) maxv=out[j*Ny+i];
		}///Nx
	}///j == Ny
	
	return maxv;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
								
/**
 * Saves the data to file
 * @return true, if success, else false
 */
bool TOccupancyGrid::save(const char *fname){
	FILE *f = fopen(fname,"wt");
	if(f==NULL){
		fprintf(stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	fprintf(f,"%d %d %f %f %f\n",Nx,Ny,resolution,world_pos.x,world_pos.y);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fprintf(f,"%.3f ",data[j*Nx+i]);
		}
		fprintf(f,"\n");
	}
	fclose(f); 
	return true;
}
	
/**
	* Loads the data from the file 
	*/
bool TOccupancyGrid::load(const char *fname){
	FILE *f = fopen(fname,"rt");
	if(f==NULL){
		fprintf(stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	int n = fscanf(f,"%d %d %f %f %f\n",&Nx,&Ny,&resolution,&world_pos.x,&world_pos.y);
	if(n!=5){
		fprintf(stderr,"Unable to read the header! Is '%s' an Occupancy Grid File??\n",fname);
		fclose(f);
		return false;
	}
	fprintf(stderr,"Loading Occupancy Grid:\n");
	fprintf(stderr,"Nx:%d Ny:%d Reso:%.2f Orig_x:%.2f Orig_y%.2f\n",Nx,Ny,resolution,world_pos.x,world_pos.y);
	
	mallocData();
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fscanf(f,"%f ",&data[j*Nx+i]);
		}
		fscanf(f,"\n");
	}
	fclose(f);
	return true;
}

/**
 * Saves the height map to file
 * @return true, if success, else false
 */
bool TOccupancyGrid::saveGModel(const char *fname){
	FILE *f = fopen(fname,"wt");
	if(f==NULL){
		fprintf(stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	float	size_x = Nx*resolution; 
	float size_y = Ny*resolution;
	
	///Orig x ja orig y are pixel coordinates of the upper left corner of the grid with respect to the 
	///World origin
	world_pos.x ;
	world_pos.y ;
	
	int orig_x = (int) ((world_pos.x/resolution)-Nx/2.0);
	int orig_y = (int) ((world_pos.y/resolution) + Ny/2.0);
	
	//int orig_x = -1*Nx/2;					// TODO: Find out how this is really supposed to be
	//int orig_y = Ny/2;						//			 And same here!
	
	
		
	fprintf(f,"size_x:%f\nsize_y:%f\nreso:%f\norig_x:%d\norig_y:%d\n",size_x,size_y,resolution,orig_x,orig_y);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fprintf(f,"%.3f ",data[j*Nx+i]);
		}
		fprintf(f,"\n");
	}
	fclose(f); 
	return true;
}

/**
	* Loads the data in Ground Model format
	**/
bool TOccupancyGrid::loadGModel(const char *fname){
	FILE *f = fopen(fname,"rt");
	if(f==NULL){
		fprintf(	stderr,"Unable to open file '%s'\n",fname);
		return false;
	}
	int pix_origin_x,pix_origin_y;
	float sx,sy;
	fscanf(f,"size_x:%f\n",&sx);
	fscanf(f,"size_y:%f\n",&sy);
	fscanf(f,"reso:%f\n",&resolution);
	fscanf(f,"orig_x:%d\n",&pix_origin_x);
	fscanf(f,"orig_y:%d\n",&pix_origin_y);	
	world_pos.x=0;
	world_pos.y=0;
	Nx = (int) (sx/resolution)+1;
	Ny = (int) (sx/resolution)+1;
	mallocData();
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			fscanf(f,"%f ",&data[j*Nx+i]);
		}
		fscanf(f,"\n");
	}
	
	fprintf(stderr,"Read '%s' size=(%d,%d), resolution=%.2f\n",fname,Nx,Ny,resolution);
	fclose(f);
	return true;
}

/**
 * Loads a line map (in format '%f %f %f %f\n')
 * and puts it to appropriate sized map
 * @p *fname the filename
 * @p scale  the scale for the line map (the number from the file is multiplied with this)
 * @p lineval The value that the line is set to
 * @p rescale set false if you don't want that the map is rescaled when the map is read.
 * @p reset Set true if you want to reset the grid to value @reset_val
 * @p reset val the value to reset the grid to
 * @return true if success.
 **/
bool TOccupancyGrid::loadLineMap(const char *fname, float scale,grid_t lineval,bool rescale,bool reset,grid_t reset_val){
	std::vector<Grid::TLine> lines;
	FILE *f=fopen(fname,"rt");
	if (f==NULL){
		fprintf(stderr,"File '%s'is NULL\n",fname);
		return false;
	}
	
	Grid::TLine line;
	float max=-1000;
	while (fscanf(f,"%f %f %f %f",&line.x1,&line.y1,&line.x2,&line.y2)==4){
		lines.push_back(line);
		if(fabs(line.x1)>max) max=fabs(line.x1);
		if(fabs(line.x2)>max) max=fabs(line.x2);
		if(fabs(line.y1)>max) max=fabs(line.y1);
		if(fabs(line.y2)>max) max=fabs(line.y2);
	}
	fclose(f);
	
	if(rescale){	
          Nx = (int)(2.0*max/resolution);
          Ny = (int)(2.0*max/resolution);
	 mallocData();
	}
	
	if(reset){
	 setGridToValue(reset_val);
	}
		
	fprintf(stderr,"Reset='%d', reset_val=%f scale=%f, lineval=%f\n",reset,reset_val,scale,lineval);
			
	for(int i=0;i<lines.size();i++){
		setLine(lines[i].x1,lines[i].y1,lines[i].x2, lines[i].y2, lineval);
	}
	
	return true;
}


	
